/*
 * XYController.h
 *
 *  Created on: Mar 27, 2011
 *      Author: Anton
 */

#ifndef XYCONTROLLER_H_
#define XYCONTROLLER_H_

#include "Gait.h"

class XYController {
public:
	XYController(Gait* XY) { gait = XY; }
	virtual ~XYController() { 
		delete gait;
	}
	Gait* gait;
	Gait* getGait(){ return gait; }

	int calculateIterations() {
		dx = sin((desiredY - currentY) / sqrt(pow((desiredX - currentX), 2) + pow((desiredY - currentY), 2)) * gait->getWidth() / 2 );
		dy = sin((desiredX - currentX) / sqrt(pow((desiredX - currentX), 2) + pow((desiredY - currentY), 2)) * gait->getLength() / 2);
		a = sqrt(pow((desiredX - currentX), 2) + pow((desiredY - currentY), 2));
		da = sqrt(pow(dx, 2) + pow(dy, 2));

		i = (int) (ceil(a / da));
		return i;
	}

	void calculateVectors(){
		gait->rbe->setFl( sin((desiredY - currentY) / sqrt(pow((desiredX - currentX), 2.0) + pow((desiredY - currentY), 2.0))) );
		gait->rbe->setBl( sin((desiredY - currentY) / sqrt(pow((desiredX - currentX), 2.0) + pow((desiredY - currentY), 2.0))) );
		gait->rbe->setVl( sin((desiredX - currentX) / sqrt(pow((desiredX - currentX), 2.0) + pow((desiredY - currentY), 2.0))) );
	}

	void setCurrentX(double X) { currentX = X; } 
	void setCurrentY(double Y) { currentY = Y; } 
	void setDesiredX(double X) { desiredX = X; } 
	void setDesiredY(double Y) { desiredY = Y; } 
	double getCurrentX() { return currentX; }
	double getCurrentY() { return currentY; }
	double getDesiredX() { return desiredX; }
	double getDesiredY() { return desiredY; }
	int getI() const { return i; }
	void setI(int i) { this->i = i; }
	double getA() { return a; }
	double getDa() { return da; }
	double getDx() { return dx; }
	double getDy() { return dy; }
private:
	double dx;
	double dy;
	double currentX;
	double currentY;
	double desiredX;
	double desiredY;
	int i;
	double a;
	double da;
};

#endif /* XYCONTROLLER_H_ */
